//
// Created by 张祎鹤严 on 2022/12/10.
//
#include "task_lostcounter.h"
#include "driver_chassis.h"
#include "cmsis_os2.h"
#include "iwdg.h"
uint32_t LostCounterCountNumber[NUMBERS_OF_COUNT];
void LostCounterInit(void)											// LostCounterCountNumber[i]初始化值为1000
{
    uint8_t i;
    for(i = 0; i < NUMBERS_OF_COUNT; i++){
        LostCounterCountNumber[i]=1000;
    }
}
void LostCounterFeed(uint8_t i)
{
    LostCounterCountNumber[i] = 0;
}
void LostCounterCount(void)
{
    for (int i = 0; i < NUMBERS_OF_COUNT; ++i) {
        LostCounterCountNumber[i] += 0x00000001;
    }
}
void LostCounterControl(uint16_t SystemErrorStatus)
{
    if((SystemErrorStatus >> REMOTE_LOST_COUNT) & 0x0001)
    {
        chassisControlData.chassisMode = CHASSIS_MODE_FREE;//关控保护
    }

    LostCounterCount();
}

/********************************************************************************************
	SystemErrorStatus:
  				|	    		|   	      	|   	      	|   	   		|				|				|   备注
 				|		1		|   	12  	|   	11  	|   	10		|		9		|		8		|   优先级
		15		|		14		|   	13  	|   	12  	|   	11		|		10		|		9		|   ID
				|	        	|	  Vision 	|	  SuperC	|      Yaw 		|	  Pitch 	|	FRICTION	|   LostCounter
				|	云台底盘通信	|	   图传		|	   电容		|	 Yaw电机		|	Pitch电机	| 	  摩擦轮	    |   失联位置
---------------------------------------------------------------------------------------------
					两个及以上失联优先级为7（底盘）					|	    		| 	    		|	    		|	        	|	   	    	|   备注
   		6		|		6		|		6		|   	6		|   	5  		|   	4   	|   	3   	|		2   	|   	1  		|   优先级
  		8		|		7		|		6		|   	5		|   	4  		|   	3   	|   	2   	|		1   	|   	0  		|   ID
	  MOTOR-3	|	 MOTOR-2	| 	MOTOR-1 	|	  MOTOR-0	|	FEED_MOTOR	|	   HEAT 	|	  BSPEED	|  	JUDGEMENT  	|	  REMOTE	|   LostCOunter
							底盘电机							    |	  拨弹		| 	   热量		|	   射速		|	 裁判系统	|	   遥控		|   失联位置
********************************************************************************************/
uint16_t GetErrorState(void)
{
    uint16_t SystemErrorStatus = 0;

    if(LostCounterCountNumber[REMOTE_LOST_COUNT]>(REMOTE_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << REMOTE_LOST_COUNT;

    if(LostCounterCountNumber[JUDGEMENT_LOST_COUNT]>(JUDGEMENT_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= (1 << JUDGEMENT_LOST_COUNT);

    if(LostCounterCountNumber[FEEDMOTOR_LOST_COUNT]>(FEEDMOTOR_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << FEEDMOTOR_LOST_COUNT;

    if(LostCounterCountNumber[CHASSIS_MOTOR_0]>(CHASSIS_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << CHASSIS_MOTOR_0;

    if(LostCounterCountNumber[CHASSIS_MOTOR_1]>(CHASSIS_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << CHASSIS_MOTOR_1;

    if(LostCounterCountNumber[CHASSIS_MOTOR_2]>(CHASSIS_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << CHASSIS_MOTOR_2;

    if(LostCounterCountNumber[CHASSIS_MOTOR_3]>(CHASSIS_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << CHASSIS_MOTOR_3;

    if(LostCounterCountNumber[L_FRICTION_LOST_COUNT]>(GIMBAL_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << L_FRICTION_LOST_COUNT;

    if(LostCounterCountNumber[R_FRICTION_LOST_COUNT]>(GIMBAL_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << R_FRICTION_LOST_COUNT;

    if(LostCounterCountNumber[H_FRICTION_LOST_COUNT]>(GIMBAL_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << H_FRICTION_LOST_COUNT;

    if(LostCounterCountNumber[GIMBAL_MOTOR_PITCH]>(GIMBAL_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << GIMBAL_MOTOR_PITCH;

    if(LostCounterCountNumber[GIMBAL_MOTOR_YAW]>(GIMBAL_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << GIMBAL_MOTOR_YAW;

    if(LostCounterCountNumber[SUPERC_LOST_COUNT]>(JUDGEMENT_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << SUPERC_LOST_COUNT;

    if(LostCounterCountNumber[VISION_LOST_COUNT]>(GIMBAL_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << VISION_LOST_COUNT;

#if NO_HERO == 0
    if(LostCounterCountNumber[SMALL_PITCH_COUNT]>(GIMBAL_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << SMALL_PITCH_COUNT;
    if(LostCounterCountNumber[SCOPE_LOST_COUNT]>(GIMBAL_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        SystemErrorStatus |= 1 << SCOPE_LOST_COUNT;
#endif

    return SystemErrorStatus;
}

void LostCounterTask(void *argument)
{
    /* USER CODE BEGIN LostCounterTask */
    LostCounterInit();
    /* Infinite loop */
    for(;;)
    {
        LostCounterControl(GetErrorState());

#if CONFIG_USE_IWDG
        if(gimbalControlData.resetFlag != 1)
            HAL_IWDG_Refresh(&hiwdg);
#endif

        osDelay(50);
    }
    /* USER CODE END LostCounterTask */
}